#include <stdio.h>
#include <iostream>
#include <algorithm>
#include "../inc/PointCloud.h"

#ifdef __APPLE__
        #include <OpenGL/gl.h>
        #include <OpenGL/glu.h>
        #include <GLUT/glut.h>
#else
	#ifdef _WIN32
        #include "../glut.h"
	#else
        #include <GL/gl.h>
        #include <GL/glu.h>
        #include <GL/glut.h>
	#endif
#endif

using namespace std;

bool compare (Point _p1, Point _p2)
{
	return (_p1.getZ() < _p2.getZ());
}

float Distance(Point _p1, Point _p2)
{
	return sqrt(pow(_p2.getX()-_p1.getX(), 2) + pow(_p2.getY()-_p1.getY(), 2));
}

PointCloud::PointCloud (void)
{

}	

PointCloud::PointCloud(const PointCloud & _c)
{
	this->ListePoints = _c.ListePoints;
}

PointCloud::~PointCloud()
{
	this->ListePoints.clear();
}

void PointCloud::add(Point _p)
{
	this->ListePoints.push_back(_p);
}

Point PointCloud::get(int _i)
{
	return this->ListePoints[_i];
}

void PointCloud::clear(void)
{
	this->ListePoints.clear();
}

int PointCloud::size(void)
{
	return this->ListePoints.size();
}

void PointCloud::getCircles(vector <Circle> & _tabCercles)
{
	float radiusMax = 1.0;
	Point p1, p2, p3;
	Circle c;

	if ( this->size() >= 3 )
	{
		for(int i=0; i<this->size()-2; i++)
		{
			p1 = this->get(i);
	
			for(int j=i+1; j<this->size()-1; j++)
			{
				p2 = this->get(j);
	
				for(int k=j+1; k<this->size(); k++)
				{
						p3 = this->get(k);
						c = Circle(p1, p2, p3);
						if(c.getR() < radiusMax )
						{
							_tabCercles.push_back(c);
						}
				}
			}
		}
	}
}

void PointCloud::getCircles(HoughSpace & _hs, float _rMax)
{
	int a, b, r;
	Point p1, p2, p3;
	Circle c;

	if ( this->size() >= 3 )
	{
		for(int i=0; i<this->size()-2; i++)
		{
			p1 = this->get(i);
	
			for(int j=i+1; j<this->size()-1; j++)
			{
				p2 = this->get(j);
	
				for(int k=j+1; k<this->size(); k++)
				{
					p3 = this->get(k);
					c = Circle(p1, p2, p3);
					if(c.getR() < _rMax )
					{
						/*cout<<c.getA()<<" "<<c.getB()<<" "<<c.getR()<<endl;*/
						c.setABR(c.getA() - _hs.getBot().getX(), c.getB() - _hs.getBot().getY(), c.getR() - _hs.getBot().getZ());
						
						/*cout<<c.getA()<<" "<<c.getB()<<" "<<c.getR()<<endl;*/
						
						a = abs((int)((c.getA())/_hs.getFrameSizeA()));
						b = abs((int)((c.getB())/_hs.getFrameSizeB()));
						r = abs((int)((c.getR())/_hs.getFrameSizeR()));

						if ( a >= _hs.getNbFrameA() )
							a = _hs.getNbFrameA()-1;
						
						if ( b >= _hs.getNbFrameB() )
							b = _hs.getNbFrameB()-1;
						
						if ( r >= _hs.getNbFrameR() )
							r = _hs.getNbFrameR()-1;

						_hs.increment(a,b,r);
						
						if(_hs.getScore(a, b, r) > _hs.getScoreMax())
						{
							_hs.setScoreMax(_hs.getScore(a, b, r));
							_hs.setAFrameMax(_hs.getHoughFrame(a, b, r).getBotPoint().getX() + _hs.getFrameSizeA()/2);
							_hs.setBFrameMax(_hs.getHoughFrame(a, b, r).getBotPoint().getY() + _hs.getFrameSizeB()/2);
							_hs.setRFrameMax(_hs.getHoughFrame(a, b, r).getBotPoint().getZ() + _hs.getFrameSizeR()/2);
						}
					}
				}
			}
		}
	}
}

void PointCloud::getCircles(HoughSpace & _hs, float _rMax, int _coef)
{
	int a, b, r;
	Point p1, p2, p3;
	Circle c;

	if ( this->size() >= 3 )
	{
		for(int i=0; i<this->size()-2; i++)
		{
			p1 = this->get(i);
	
			for(int j=i+1; j<this->size()-1; j++)
			{
				p2 = this->get(j);
	
				for(int k=j+1; k<this->size(); k++)
				{
					p3 = this->get(k);
					c = Circle(p1, p2, p3);
					
					if(c.getR() < _rMax )
					{
						/*cout<<c.getA()<<" "<<c.getB()<<" "<<c.getR()<<endl;*/
						c.setABR(c.getA() - _hs.getBot().getX(), c.getB() - _hs.getBot().getY(), c.getR() - _hs.getBot().getZ());
						
						/*cout<<c.getA()<<" "<<c.getB()<<" "<<c.getR()<<endl;*/
						
						a = abs((int)((c.getA())/_hs.getFrameSizeA()));
						b = abs((int)((c.getB())/_hs.getFrameSizeB()));
						r = abs((int)((c.getR())/_hs.getFrameSizeR()));

						if ( a >= _hs.getNbFrameA() )
							a = _hs.getNbFrameA()-1;
						
						if ( b >= _hs.getNbFrameB() )
							b = _hs.getNbFrameB()-1;
						
						if ( r >= _hs.getNbFrameR() )
							r = _hs.getNbFrameR()-1;

						_hs.increment(a, b, r, _coef);
						
						if(_hs.getScore(a, b, r) > _hs.getScoreMax())
						{
							_hs.setScoreMax(_hs.getScore(a, b, r));
							_hs.setAFrameMax(_hs.getHoughFrame(a, b, r).getBotPoint().getX() + _hs.getFrameSizeA()/2);
							_hs.setBFrameMax(_hs.getHoughFrame(a, b, r).getBotPoint().getY() + _hs.getFrameSizeB()/2);
							_hs.setRFrameMax(_hs.getHoughFrame(a, b, r).getBotPoint().getZ() + _hs.getFrameSizeR()/2);
						}
					}
				}
			}
		}
	}
}

PointCloud &PointCloud::operator= (const PointCloud &_c)
{
        this->ListePoints = _c.ListePoints;
        return (*this);
}

void PointCloud::displayPointCloud ()
{
	glBegin ( GL_POINTS );
	
	for ( int i = 0 ; i < this->size() ; i++ )
	{
		glVertex3f ( this->ListePoints[i].getX(), this->ListePoints[i].getY(), this->ListePoints[i].getZ() );
	}

	glEnd();
}

void PointCloud::displayPointCloud ( float _r, float _g, float _b)
{
	glColor3f(_r,_g,_b);
	glBegin ( GL_POINTS );
	
	for ( int i = 0 ; i < this->size() ; i++ )
	{
		glVertex3f ( this->ListePoints[i].getX(), this->ListePoints[i].getY(), this->ListePoints[i].getZ() );
	}

	glEnd();
	glColor3f(1,1,1);
}

void PointCloud::printPointCloud ()
{
	for ( int i = 0 ; i < this->size() ; i++ )
		cout<<"Sommet "<<i<<" : "<<this->ListePoints[i].getX()<<" "<<this->ListePoints[i].getY()<<" "<<this->ListePoints[i].getZ()<<endl;
}

float PointCloud::getZMin()
{
	float zMin = 10000;
	
	for ( int i = 0 ; i < this->size() ; i++ )
	{
		if ( zMin > this->ListePoints[i].getZ() )
			zMin = this->ListePoints[i].getZ();
	}
	
	return zMin;
}

Point PointCloud::getPointMin()
{
	float zMin = 10000;
	Point rslt;
	for ( int i = 0 ; i < this->size() ; i++ )
	{
		if ( zMin > this->ListePoints[i].getZ() )
			rslt = this->ListePoints[i];
	}
	
	return rslt;
}

/* *********************************************************************************************************** */
/*	FONCTION A REVOIR PEUT ETRE : PASSAGE PAR POINTEUR OU QUOI POUR EVITER DE RECOPIER BETEMENT RSLT DANS THIS */
/* *********************************************************************************************************** */

void PointCloud::keepPointsBetween(float _zMin, float _zMax)
{
	PointCloud rslt;
	
	rslt.clear();
	
	for ( int i = 0 ; i < this->size() ; i++ )
		if ( this->ListePoints[i].getZ() > _zMin && this->ListePoints[i].getZ() < _zMax )
			rslt.add( this->ListePoints[i] );
	
	this->clear();
	
	for ( int i = 0 ; i < rslt.size() ; i++ )
		this->add( rslt.ListePoints[i] );
}

void PointCloud::setOrigin ( float _xMin, float _yMin, float _zMin )
{
	for ( int i = 0 ; i < this->size() ; i++ )
	{
		this->ListePoints[i].setX( this->ListePoints[i].getX() - _xMin );
		this->ListePoints[i].setY( this->ListePoints[i].getY() - _yMin );
		this->ListePoints[i].setZ( this->ListePoints[i].getZ() - _zMin );
	}
}

void PointCloud::keepSlice(float _bas, float _haut)
{
	float zMin = this->getZMin();
	PointCloud rslt;
	
	rslt.clear();
	
	for ( int i = 0 ; i < this->size() ; i++ )
	{
		if ( ( this->ListePoints[i].getZ() > zMin + _bas ) && ( this->ListePoints[i].getZ() < zMin + _haut ))
			rslt.add( this->ListePoints[i] );
	}
	
	this->clear();
	
	for ( int i = 0 ; i < rslt.size() ; i++ )
		this->add( rslt.ListePoints[i] );
}

void PointCloud::keepSlice(float _bas, int _nbPoints )
{
	int i = 0;
	float zMin;
	PointCloud rslt;
	
	rslt.clear();
	
	this->Sort();
	
	if ( this->size() > 0 )
	{
		zMin = this->ListePoints[0].getZ();
		
		while ( this->ListePoints[i].getZ() < zMin+_bas && i < this->size() )
			i++;
		
		for ( int j = i ; j < this->size() && j < _nbPoints + i ; j++ )
			rslt.add( this->get(j) );
	}
	
	this->clear();
	
	for ( int i = 0 ; i < rslt.size() ; i++ )
		this->add( rslt.ListePoints[i] );
}

void PointCloud::keepSlice(float _bas, float _haut, int _nbPoints )
{
	int i = 0;
	float zMin;
	PointCloud rslt;
	
	rslt.clear();
	
	if ( this->size() > 0 )
	{
		this->Sort();
		
		zMin = this->ListePoints[0].getZ();
		
		while ( this->ListePoints[i].getZ() < zMin+_bas && i < this->size() )
			i++;
		
		for ( int j = i ; j < this->size() && j < _nbPoints + i && this->ListePoints[j].getZ() < zMin+_haut ; j++ )
			rslt.add( this->get(j) );
	}
	
	this->clear();
	
	if ( rslt.size() != 0 )
	{
		for ( int i = 0 ; i < rslt.size() ; i++ )
			this->add( rslt.ListePoints[i] );
	}
	
	rslt.clear();
}

PointCloud PointCloud::getSlice(float _bas, float _haut)
{
	float zMin = this->getZMin();
	PointCloud rslt;
	
	rslt.clear();
	
	for ( int i = 0 ; i < this->size() ; i++ )
	{
		if ( ( this->ListePoints[i].getZ() > zMin + _bas ) && ( this->ListePoints[i].getZ() < zMin + _haut ))
			rslt.add( this->ListePoints[i] );
	}
	return rslt;
}

PointCloud PointCloud::getSlice(float _bas, int _nbPoints)
{
	int i = 0;
	float zMin;
	PointCloud rslt;
	
	rslt.clear();
	
	this->Sort();
	
	if ( this->size() > 0 )
	{
		zMin = this->ListePoints[0].getZ();
		
		while ( this->ListePoints[i].getZ() < zMin+_bas && i < this->size() )
			i++;
		
		for ( int j = i ; j < this->size() && j < _nbPoints + i ; j++ )
			rslt.add( this->get(j) );
	}
	return rslt;
}

PointCloud PointCloud::getSlice(float _bas, float _haut, int _nbPoints)
{
	int i = 0;
	float zMin;
	PointCloud rslt;
	
	rslt.clear();
	
	this->Sort();
	
	if ( this->size() > 0 )
	{
		zMin = this->ListePoints[0].getZ();
		
		while ( this->ListePoints[i].getZ() < zMin+_bas && i < this->size() && this->ListePoints[i].getZ() < _haut )
			i++;
		
		for ( int j = i ; j < this->size() && j < _nbPoints + i ; j++ )
			rslt.add( this->get(j) );
	}
	return rslt;
}

void PointCloud::Sort()
{
	sort(ListePoints.begin(), ListePoints.end(), compare);
}
